package edu.bmstu.rslab;

import java.awt.geom.Point2D;

/**
 * Created by Chikh (using IntelliJ IDEA).
 * Date: 23.04.2009
 * Time: 14:14:37
 */
public class MobileRobotStubImpl implements MobileRobot{

    private final double SIZE = 0.05;
    private double velocity = 0;
    private double rotationSpeed;
    private double acceleration;

    public void updateBot() {
        rotateBot();
        accelerateBot();
        currentPosition = new Point2D.Double(currentPosition.getX() + velocity*directionCos,
                currentPosition.getY() + velocity*directionSin);
    }

    public void turnAnticlockwise(double angle) {
        rotationSpeed = angle / 5.0;
    }

    public void setVelocity(double velocity) {
        this.acceleration = velocity - this.velocity;
    }

    private void accelerateBot() {
        velocity = velocity + acceleration;
        acceleration = 0;
    }

    private void rotateBot()
    {
        double angleCos = Math.cos(rotationSpeed);
        double angleSin = Math.sin(rotationSpeed);
        double directionCosPrevious = directionCos;
        directionCos = directionCosPrevious*angleCos - directionSin*angleSin;
        directionSin = directionSin*angleCos + directionCosPrevious*angleSin;
    }

    public void stop() {
        velocity = 0;
        rotationSpeed = 0;
    }

    public Point2D getCurrentPosition() {
        if (currentPosition == null) {
            return null;
        }
       return (Point2D)currentPosition.clone();
    }

    public Point2D getCurrentDirectionPoint() {
        if (!(positionSet)) {
            return null;
        }
       return new Point2D.Double(currentPosition.getX()+SIZE*directionCos,
               currentPosition.getY()+SIZE*directionSin);
    }

    public void setPosition(Point2D position) {
        if (!(positionSet)) {
            currentPosition = (Point2D)position.clone();
            positionSet = true;
        }
    }

    public void setDirection(double direction) {
        if ((positionSet) && !(directionSet)) {
            this.directionCos = Math.cos(direction);
            this.directionSin = Math.sin(direction);
            positionSet = true;
            directionSet = true;
        }
    }

    private Point2D currentPosition;
    private double directionCos;
    private double directionSin;
    private boolean positionSet = false;
    private boolean directionSet = false;
}
